//vel2pwm节点，订阅/cmd_vel，发布/pwm。
//有用的数据仅为x轴速度和z轴角速度
// 打角115为正，160左40度，70右40度
// 速度1520不动，1525为0.2但有响声，1530为0.6，1540为0.7，1550为1.0，1560可到1.8左右不过加速还要一段距离，1565为2左右

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

class vel2pwm
{
private:
    ros::NodeHandle nh;
    ros::Subscriber sub;
    ros::Publisher pub;
public:
    vel2pwm()
    {
        sub = nh.subscribe("/cmd_vel",1,&vel2pwm::TwistCallback,this);
        pub = nh.advertise<geometry_msgs::Twist>( "/pwm", 1 );
    }
    // ~vel2pwm();
    void TwistCallback(const geometry_msgs::Twist& twist)
    {
        geometry_msgs::Twist pwm;
        pwm.linear.x=1555;
        if(twist.angular.z >= 0.7) pwm.angular.z = 160; //限幅左右40度
        else if(twist.angular.z <= -0.7) pwm.angular.z = 70;
        else pwm.angular.z=twist.angular.z*64.46+115; //*180/pi*45/40，即弧度制转角度制再转pwm
        pub.publish(pwm);
    }
};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "vel2pwm");
    vel2pwm vel2pwm_;
    ROS_INFO("vel2pwm start successfully.");
    ros::spin();
    return 0;
}